/*
 * @brief Sensor Hub demo sensors (implementation file)
 *
 * @note
 * Copyright(C) NXP Semiconductors, 2015
 * All rights reserved.
 *
 * @par
 * Software that is described herein is for illustrative purposes only
 * which provides customers with programming information regarding the
 * LPC products.  This software is supplied "AS IS" without any warranties of
 * any kind, and NXP Semiconductors and its licensor disclaim any and
 * all warranties, express or implied, including all implied warranties of
 * merchantability, fitness for a particular purpose and non-infringement of
 * intellectual property rights.  NXP Semiconductors assumes no responsibility
 * or liability for the use of the software, conveys no license or rights under any
 * patent, copyright, mask work right, or any other intellectual property rights in
 * or to any products. NXP Semiconductors reserves the right to make changes
 * in the software without notification. NXP Semiconductors also makes no
 * representation or warranty that such application will be suitable for the
 * specified use without further testing or modification.
 *
 * @par
 * Permission to use, copy, modify, and distribute this software and its
 * documentation is hereby granted, under NXP Semiconductors' and its
 * licensor's relevant copyrights in the software, without fee, provided that it
 * is used in conjunction with NXP Semiconductors microcontrollers.  This
 * copyright, permission, and disclaimer notice must appear in all copies of
 * this code.
 */

#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <string.h>

#include <assert.h>
#include <math.h>

#include "hostif_protocol.h"

#include "trace.h"
#include "misc.h"

#include "sensors.h"

#define	TS_BACK		25000														// set timestamp backstep report at 25ms
#define	TS_JUMP		1500000														// set timestamp jump report at 1500ms

//------------------------------------------------------------------------------
// *	These are the sensors (by sensor ID) that are
// *	supported by the BSX-lite library:
// *	- LPCSH_SENSOR_ID_ACCELEROMETER
// *	- LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED
// *	- LPCSH_SENSOR_ID_MAGNETIC_FIELD
// *	- LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED
// *	- LPCSH_SENSOR_ID_GYROSCOPE
// *	- LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED
// *	- LPCSH_SENSOR_ID_LIGHT
// *	- LPCSH_SENSOR_ID_PRESSURE (includes temperature)
// *	- LPCSH_SENSOR_ID_PROXIMITY
// *	- LPCSH_SENSOR_ID_ORIENTATION
// *	- LPCSH_SENSOR_ID_ROTATION_VECTOR
// *
// * Note:
// *	The rotation vector sensor is a special case.
// *	It drives the teapot display and can have two different HUD displays
// *	the rotation vector and the rotation matrix.
//------------------------------------------------------------------------------
static	struct lpcsh_motion_sensor_node			m_accel_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_accel_uncal;
static	struct lpcsh_motion_sensor_node			m_mag_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_mag_uncal;
static	struct lpcsh_motion_sensor_node			m_gyro_cal;
static	struct lpcsh_motion_uncal_sensor_node	m_gyro_uncal;
static	struct lpcsh_motion_sensor_node			m_light;
static	struct lpcsh_motion_sensor_node			m_pressure;
static	struct lpcsh_motion_sensor_node			m_proximity;
static	struct lpcsh_orientation_node			m_orientation;
static	struct lpcsh_quaternion_node			m_rotation;

static	uint32_t	timestamp;
static	bool		ts_format = true;
static uint32_t		ts_save;
static bool			ts_init = false;

//------------------------------------------------------------------------------
//	Set the file handle used by all the output
//------------------------------------------------------------------------------
static FILE* fh_csv;
void set_file(FILE* f)
{
	fh_csv = f;
	fprintf(fh_csv, "time, sensor, data x, data y, data z\n");
}

//------------------------------------------------------------------------------
//	Time stamp assert: make sure timestamp is reasonable
//------------------------------------------------------------------------------
static void ts_assert(uint32_t ts)
{
	if (ts_init == false) {														// Initialization variable
		ts_init = true;															// Set to true (do this once)
		ts_save = ts;															// Initialize the timestamp
	}

	if (ts == 0)																// Check if timestamp is zero
		trace(TRACE_INFO, "timestamp zero");									// if so, trace it
	if (ts < ts_save-TS_BACK)													// Check if timestamp has jumped backward more than 1ms
		trace(TRACE_INFO, "timestamp backstep: %d", abs(ts_save-ts));			// if so, trace it
	if (ts > ts_save+TS_JUMP)													// Check if timestamp has jumped forward more than 25ms
		trace(TRACE_INFO, "timestamp jump: %d", abs(ts-ts_save));				// if so, trace it

	ts_save = ts;
}

//------------------------------------------------------------------------------
//	Time stamp processing
//------------------------------------------------------------------------------
void disp_timestamp(void)
{
	if (ts_format == true)
		fprintf(fh_csv, "%s, ", time_fmt(timestamp));
	else
		fprintf(fh_csv, "%08u, ", timestamp);
}

//------------------------------------------------------------------------------
//	Accelerometer sensor (calibrated) processing
//	All the rest of the sensors follow a similar pattern.
//------------------------------------------------------------------------------
void accel_cal_init(void)
{
	memset(&m_accel_cal, 0, sizeof(m_accel_cal));								//	clear the local data structure
}
void accel_cal_acq(uint8_t* rx)
{
	uint8_t id;																	// packet ID
	struct lpcsh_sensor_node* pData;											// packet pointer

	pData = (struct lpcsh_sensor_node*)rx;										// point / cast to packet
	id = pData->header.sensorId;												// get the ID
	assert(id == LPCSH_SENSOR_ID_ACCELEROMETER);								// assert that the ID is correct
	timestamp = pData->header.timeStamp;										// get the timestamp
	ts_assert(timestamp);														// verify the timestamp
	memcpy(&m_accel_cal, &(pData->data.sensorData), sizeof(m_accel_cal));		// copy the data to a local structure
}
void accel_cal_print(void)
{
	float qx = (float)m_accel_cal.Data[0]/9.0f;									// Calculate X acceleration in g
	float qy = (float)m_accel_cal.Data[1]/9.0f;									// Calculate Y acceleration in g
	float qz = (float)m_accel_cal.Data[2]/9.0f;									// Calculate Z acceleration in g

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Accelerometer, ");											// Display heading
	fprintf(fh_csv, "%-8.3f, %-8.3f, %-8.3f\n", qx, qy, qz);					// Format data
}

//------------------------------------------------------------------------------
//	Accelerometer sensor (uncalibrated) processing
//------------------------------------------------------------------------------
void accel_uncal_init(void)
{
	memset(&m_accel_uncal, 0, sizeof(m_accel_uncal));
}
void accel_uncal_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_ACCELEROMETER_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_accel_uncal, &(pData->data.uncal_sensorData), sizeof(m_accel_uncal));
}
void accel_uncal_print(void)
{
	float qx = (float)m_accel_cal.Data[0];
	float qy = (float)m_accel_cal.Data[1];
	float qz = (float)m_accel_cal.Data[2];

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Accelerometer (uncal), ");									// Display heading
	fprintf(fh_csv, "%-8.3f, %-8.3f, %-8.3f\n", qx, qy, qz);					// Format data
}

//------------------------------------------------------------------------------
//	Magnetometer sensor (calibrated) processing
//------------------------------------------------------------------------------
void mag_cal_init(void)
{
	memset(&m_mag_cal, 0, sizeof(m_mag_cal));
}
void mag_cal_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_MAGNETIC_FIELD);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_mag_cal, &(pData->data.sensorData), sizeof(m_mag_cal));
}
void mag_cal_print(void)
{

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Magnetometer, ");											// Display heading
	fprintf(fh_csv, "%8d %8d %8d\n", m_mag_cal.Data[0], m_mag_cal.Data[1], m_mag_cal.Data[2]);
}

//------------------------------------------------------------------------------
//	Magnetometer sensor (uncalibrated) processing
//------------------------------------------------------------------------------
void mag_uncal_init(void)
{
	memset(&m_mag_uncal, 0, sizeof(m_mag_uncal));
}
void mag_uncal_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_MAGNETIC_FIELD_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_mag_uncal, &(pData->data.uncal_sensorData), sizeof(m_mag_uncal));
}
void mag_uncal_print(void)
{
	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Magnetometer (uncal), ");									// Display heading
	fprintf(fh_csv, "%8d, %8d, %8d\n", m_mag_uncal.Data[0], m_mag_uncal.Data[1], m_mag_uncal.Data[2]);
}

//------------------------------------------------------------------------------
//	Gyroscope (calibrated) processing
//------------------------------------------------------------------------------
void gyro_cal_init(void)
{
	memset(&m_gyro_cal, 0, sizeof(m_gyro_cal));
}
void gyro_cal_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_GYROSCOPE);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_gyro_cal, &(pData->data.sensorData), sizeof(m_gyro_cal));
}
void gyro_cal_print(void)
{
	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Gyroscope, ");												// Display heading
	fprintf(fh_csv, "%8d, %8d, %8d\n", m_gyro_cal.Data[0], m_gyro_cal.Data[1], m_gyro_cal.Data[2]);
}

//------------------------------------------------------------------------------
//	Gyroscope (uncalibrated) processing
//------------------------------------------------------------------------------
void gyro_uncal_init(void)
{
	memset(&m_gyro_uncal, 0, sizeof(m_gyro_uncal));
}
void gyro_uncal_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_GYROSCOPE_UNCALIBRATED);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_gyro_uncal, &(pData->data.uncal_sensorData), sizeof(m_gyro_uncal));
}
void gyro_uncal_print(void)
{
	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Gyroscope (uncal), ");										// Display heading
	fprintf(fh_csv, "%8d, %8d, %8d\n", m_gyro_uncal.Data[0], m_gyro_uncal.Data[1], m_gyro_uncal.Data[2]);
}

//------------------------------------------------------------------------------
//	Light sensor processing
//------------------------------------------------------------------------------
void light_init(void)
{
	memset(&m_light, 0, sizeof(m_light));
}
void light_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_LIGHT);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_light, &(pData->data.sensorData), sizeof(m_light));
}
void light_print(void)
{
	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Light, ");													// Display heading
	fprintf(fh_csv, "%8d\n", m_light.Data[0]);
}

//------------------------------------------------------------------------------
//	Barometric pressure processing
//------------------------------------------------------------------------------
void pressure_init(void)
{
	memset(&m_pressure, 0, sizeof(m_pressure));
}
void pressure_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_PRESSURE);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_pressure, &(pData->data.sensorData), sizeof(m_pressure));
}
void pressure_print(void)
{
	uint32_t	t_press;
	float		f_press;

	t_press = (uint32_t)((uint16_t)m_pressure.Data[1]<<16 | (uint16_t)m_pressure.Data[0]);
	f_press = (float)t_press / 100.0f;

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Pressure, ");												// Display heading
	fprintf(fh_csv, "%5.2f\n", f_press);

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Temperature, ");											// Display heading
	fprintf(fh_csv, "%5.2f\n", (float)m_pressure.Data[2]/100.0f);
}

//------------------------------------------------------------------------------
//	Proximity sensor processing
//------------------------------------------------------------------------------
void proximity_init(void)
{
	memset(&m_proximity, 0, sizeof(m_proximity));
}
void proximity_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_PROXIMITY);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_proximity, &(pData->data.sensorData), sizeof(m_proximity));
}
void proximity_print(void)
{
	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Proximity, ");												// Display heading
	fprintf(fh_csv, "%8d\n", m_proximity.Data[0]);
}

//------------------------------------------------------------------------------
//	Orientation sensor processing
//------------------------------------------------------------------------------
void orientation_init(void)
{
	memset(&m_orientation, 0, sizeof(m_orientation));
}
void orientation_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_ORIENTATION);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_orientation, &(pData->data.orientationData), sizeof(m_orientation));
}
void orientation_print(void)
{
	float qp = (float)m_orientation.Data[0] / 16777216.0f;
	float qr = (float)m_orientation.Data[1] / 16777216.0f;
	float qy = (float)m_orientation.Data[2] / 16777216.0f;

/*
 *	Convert qy to degrees from north
 */
	qy *= 360.0/(M_PI*2.0);
	qr *= 90.0/M_PI_2;
	qp *= -360.0/(M_PI*2.0);

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Orientation, ");											// Display heading
	fprintf(fh_csv, "%5.0f, %5.0f, %5.0f\n", qp, qr, qy);
}

//------------------------------------------------------------------------------
//	Rotation sensor processing
//------------------------------------------------------------------------------
void rotation_init(void)
{
	memset(&m_rotation, 0, sizeof(m_rotation));
}
void rotation_acq(uint8_t* rx)
{
	uint8_t id;
	struct lpcsh_sensor_node* pData;

	pData = (struct lpcsh_sensor_node*)rx;
	id = pData->header.sensorId;
	assert(id == LPCSH_SENSOR_ID_ROTATION_VECTOR);
	timestamp = pData->header.timeStamp;
	ts_assert(timestamp);
	memcpy(&m_rotation, &(pData->data.quaternionData), sizeof(m_rotation));
}
void rotation_print(void)
{
	float qz = (float)m_rotation.X / 16384.0f;
	float qy = (float)m_rotation.Y / 16384.0f;
	float qx = (float)m_rotation.Z / 16384.0f;

	disp_timestamp();															// Display time-stamp
	fprintf(fh_csv, "Rotation vector, ");										// Display heading
	fprintf(fh_csv, "%8.3f, %8.3f, %8.3f\n", qx, qy, qz);
}

//------------------------------------------------------------------------------
//	Fake/Padding sensor processing
//------------------------------------------------------------------------------
void fake_init(void)
{
}
void fake_acq(uint8_t* rx)
{
}
void fake_print(void)
{
}
